//#include "serial_node.hpp"
#include "bianma.hpp"
#include "yaogan.hpp"
#include "wheelctr.hpp"
#include <chrono>
#include <memory>

#include <string>
#include <stdlib.h>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

#include "wheel_msg/msg/wheelout.hpp"
#include "wheel_msg/msg/wheelin.hpp"
#include "wheel_module.hpp"
#include "yaml-cpp/yaml.h"

using namespace std::chrono_literals;
using namespace std;
class Serial_Publisher : public rclcpp::Node
{
public:
  Serial_Publisher()
  : Node("yaokong_publisher"), count_(0)
  {
   publisher_ = this->create_publisher<wheel_msg::msg::Wheelout>("eight_wheel_info", 10);
  // publisher_ = this->create_publisher<serial_all::msg::Num>("yaogan", 10);
  // publisher_ = this->create_publisher<Yao_Gan>("yaogan", 10);
   subscription_ = this->create_subscription<wheel_msg::msg::Wheelin>(  \
      "yaokong", 20, std::bind(&Serial_Publisher::topic_callback, this, std::placeholders::_1)); 
    timer_ = this->create_wall_timer(
      10ms, std::bind(&Serial_Publisher::timer_callback, this));
    bm=new bianma("ttyS7",1000000);
    yg=new yaogan("ttyS6",115200);
    wlc=new wheelctr("ttyS4",1000000);
    w_module1=new wheel_module();
    w_module2=new wheel_module();
    w_module3=new wheel_module();
    w_module4=new wheel_module();
    //for(int i=0;i<sizeof(msg_set);i++){ msg_set.msg[i]=0;msg_cur.msg[i]=0;}
    test_flag=1;
    work_mode=1;
    // read_yaml();
  }
 // void read_yaml(void); 
private:
//wheelin -->amsg
  void topic_callback(const wheel_msg::msg::Wheelin::SharedPtr amsg) //const
    {
      work_mode=amsg->motorother[0];
     switch(amsg->motorother[0])
     {
      case 0:
          for(int i=0;i<8;i++)
            wlc->set_speed[i]=amsg->speedset[i];
          RCLCPP_INFO(this->get_logger(), "receive ctr ");
       break;
      case 1:
        w_module1->set_module_speed(amsg->speedset[0]);
        w_module2->set_module_speed(amsg->speedset[1]);
        w_module3->set_module_speed(amsg->speedset[2]);
        w_module4->set_module_speed(amsg->speedset[3]);
        w_module1->set_module_angle(-amsg->angleset[0]);
        w_module2->set_module_angle(-amsg->angleset[1]);
        w_module3->set_module_angle(-amsg->angleset[2]);
        w_module4->set_module_angle(-amsg->angleset[3]);
      //  w_module1->set_module_angle(5400);
      //   w_module2->set_module_angle(5400);
      //   w_module3->set_module_angle(5400);
      //   w_module4->set_module_angle(5400);
        // for(int i=0;i<4;i++)
        //   {
          //  if(amsg->speedset[i]!=0) RCLCPP_INFO(this->get_logger(), "speed= %dilun=%d",i,amsg->speedset[i]);
          //  if(amsg->angleset[i]!=0) RCLCPP_INFO(this->get_logger(), "ang=  %d iagl=%d",i,amsg->angleset[i]);
          // }
        Data_swap();
        break;    
     }  
    }
  void timer_callback()
  {
   // RCLCPP_INFO(this->get_logger(), "I heard: %d %d",yg->yg_value.Left_horiz,yg->yg_value.Left_vertic); 
   
    auto msg=wheel_msg::msg::Wheelout();
    for(int i=0;i<8;i++)
      {
        msg.speedcur[i]=wlc->current_speed[i];
      }
    for(int j=0;j<4;j++)
    {
      msg.anglecur[j]=bm->current_angle[j];
    }  
   //  RCLCPP_INFO(this->get_logger(), "111aa=%d  fr_angle_n=%d",  msg.anglecur[0] , msg.anglecur[1] );
    yaogan_timer_count++;
    if(yaogan_timer_count>300)  //摇杆超时判断
    {
      yaogan_timer_count=0;
     if(2>yg->get_rec_count())
       {
        stop_flag=1;

       }
       else stop_flag=0;
    }
    if(stop_flag==1)
    {
        msg.yaogan[0]=0; msg.yaogan[1]=0;
        msg.yaogan[2]=0; msg.yaogan[3]=0;
        msg.otherout[1]=0; msg.otherout[2]=0;
        publisher_->publish(msg);
       }
      else
       {
        msg.yaogan[0]=yg->yg_value.Left_horiz; msg.yaogan[1]=yg->yg_value.Left_vertic;
        msg.yaogan[2]=yg->yg_value.Right_horiz; msg.yaogan[3]=yg->yg_value.Right_vertic;
        msg.otherout[1]=yg->yg_value.key1; msg.otherout[2]=yg->yg_value.key2;
        publisher_->publish(msg);
       }  
    
  //  RCLCPP_INFO(this->get_logger(), "angle= %d s= %d %d %d ",bm->current_angle[0],bm->current_angle[1],bm->current_angle[2],bm->current_angle[3]);
    switch(work_mode)
    {
      case 0:
      //   Data_swap();
      //  RCLCPP_INFO(this->get_logger(), "yaogan k1=%x k2=%x",yg->yg_value.Left_vertic,yg->yg_value.Right_horiz);
      //   Run_yaokong();  //执行遥控器命令
        break;
      case 1:
      // Data_swap();
        break;  
    }
    Data_swap();
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<wheel_msg::msg::Wheelout>::SharedPtr publisher_;
  rclcpp::Subscription<wheel_msg::msg::Wheelin>::SharedPtr subscription_;
 // rclcpp::Publisher<serial_all::msg::Num>::SharedPtr publisher_;
  size_t count_;
  // yaogan yg("ttyS8",115200);
  yaogan *yg;
  bianma *bm;
  wheelctr *wlc;
  wheel_module *w_module1,*w_module2,*w_module3,*w_module4;
  //msgvalue msg_set,msg_cur;
  int test_flag,work_mode;
  void Run_yaokong(void);
  void Data_swap(void);
  int yaogan_timer_count=0;
   int stop_flag=0;
};
void Serial_Publisher::Run_yaokong(void)
{
  YG_value *yaogan_temp;
  int16_t speed,angle;
  if(yg->Get_yaogan_flag()==1)
  {
    yaogan_temp=yg->get_yaogan_ptr();
    if(yaogan_temp->key2==5)
      {bm->Set_reset_zero(); //编码器归零
       return ;
      }
      else bm->Clr_reset_zero();
    speed=yaogan_temp->Left_vertic*24;
    w_module1->set_module_speed(speed);
    w_module2->set_module_speed(speed);
    w_module3->set_module_speed(speed);
    w_module4->set_module_speed(speed);
    angle=-yaogan_temp->Right_horiz*70;
    w_module1->set_module_angle(angle);
    w_module2->set_module_angle(angle);
    w_module3->set_module_angle(angle);
    w_module4->set_module_angle(angle);
    
  }
} 
void Serial_Publisher::Data_swap(void)
{
  w_module1->set_angle_cur(bm->current_angle[0]);
  w_module1->conclude();
  wlc->set_speed[0]=w_module1->speed_set_L;
  wlc->set_speed[1]=w_module1->speed_set_R;

  w_module2->set_angle_cur(bm->current_angle[1]);
  w_module2->conclude();
  wlc->set_speed[2]=w_module2->speed_set_L;
  wlc->set_speed[3]=w_module2->speed_set_R;

  w_module3->set_angle_cur(bm->current_angle[2]);
  w_module3->conclude();
  wlc->set_speed[4]=w_module3->speed_set_L;
  wlc->set_speed[5]=w_module3->speed_set_R;

  w_module4->set_angle_cur(bm->current_angle[3]);
  w_module4->conclude();
  wlc->set_speed[6]=w_module4->speed_set_L;
  wlc->set_speed[7]=w_module4->speed_set_R;

 // RCLCPP_INFO(this->get_logger(), "I send L=%d r=%d",w_module1->speed_set_L,w_module1->speed_set_R);
}

// void Serial_Publisher::read_yaml(void)
// {
//         const auto fleet_config = YAML::LoadFile("/home/orangepi/json_tmp/my.yaml");
//       const YAML::Node rmf_fleet = fleet_config["name"];
//      if (!rmf_fleet)RCLCPP_INFO(this->get_logger(),"no find my.yaml!");
//      if (!rmf_fleet["name"])RCLCPP_INFO(this->get_logger(),"no find name in my.yaml!");
//       else 
//       {
//         std::cout<<rmf_fleet["name"]<<std::endl;
//       }
// }






int main(int argc, char * argv[])
{
     
        const auto fleet_config = YAML::LoadFile("/home/orangepi/eight_wheel/src/serial_all/config.yaml");
      const YAML::Node rmf_fleet = fleet_config["name"];
      std::cout << "name: " << rmf_fleet.as<std::string>() << std::endl;


        // const YAML::Node r_fleet = fleet_config["value"];
        // int value=r_fleet.as<int>;
       //std::cout<< "value"<<value[1]<<std::endl;


        const YAML::Node ar_fleet = fleet_config["age"];
        int age=ar_fleet.as<int>();
        std::cout<<"age=" <<age<<endl;
     
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<Serial_Publisher>());
  rclcpp::shutdown();
  return 0;
}


